#include "ros/ros.h"
//#define build_full

#include "FeatureDescriptor.cpp"
#include "Histogram.cpp"
#include "RGBFeatureDescriptor.cpp"

#include "Point.cpp"
#include "Plane.cpp"
#include "Line.cpp"

#ifdef build_full
	#include "cvap_6D_slam/kinect_data.h" // kinect_data msg
	#include <string.h>
	//Slam files
	//#include "Point.cpp"
	//#include "Plane.cpp"
	#include "RGBDCamera.cpp"
	#include "RGBDFrame.cpp"
	#include "KeyPoint.cpp"
	#include "KeyPointSet.cpp"
	#include "FeatureExtractor.cpp"
	#include "SurfExtractor.cpp"
	#include "FrameNode.cpp"
	#include "Transformation.cpp"

	#include "graph/VertexCamera.cpp"
	#include "graph/VertexPlane.cpp"
	#include "graph/VertexPoint3D.cpp"

	#include "graph/EdgeCameraPlane.cpp"

	#include "Map.cpp"
	#include "AssociationSet.cpp"
	#include "Association.cpp"
	#include "PlaneAssociation.cpp"

	#include "FrameMatcher.cpp"
	#include "SurfRansacMatcherI.cpp"
	#include "PlanePointRansacMatcherI.cpp"
#endif
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/io.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
//using namespace cvap_6D_slam;

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>

#include <sensor_msgs/PointCloud2.h>
//OpenCV
#include "cv.h"
#include "highgui.h"

#include "segmentation.cpp"

#ifdef build_full

RGBDCamera * camera;
cvap_6D_slam::Map * mymap;
boost::shared_ptr<pcl::visualization::PCLVisualizer> * view;

void chatterCallback(const cvap_6D_slam::kinect_data::ConstPtr& msg)
{
	ROS_INFO("I heard: timing:%f, rgb path: %s, depth path: %s]",(double) msg->timing, msg->rgb.c_str(),msg->depth.c_str());
	camera->addFrame(msg);
	ROS_INFO("part2");
	RGBDFrame *  frame = camera->getLastFrame();
	//delete frame;
	mymap->addFrame(frame);
	//view->showCloud(frame->getCloud());
}

#endif
int c = 0;

void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &input)
{
	//ROS_INFO("recived a pointcloud");
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::fromROSMsg (*input, *cloud);
	
	struct timeval start, end;
	gettimeofday(&start, NULL);
	
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
	//ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
	//ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
	ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
	ne.setMaxDepthChangeFactor(0.02f);
	ne.setNormalSmoothingSize(5.0f);
	ne.setInputCloud(cloud);
	ne.compute(*normals);

	gettimeofday(&end, NULL);
	ROS_INFO("Normal estimation time: %f",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr full_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
	pcl::concatenateFields(*cloud,*normals,*full_cloud);
	
	#ifdef build_full
	//if(c%30 == 0){
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> cloud_rgb(cloud);
		(*view)->removePointCloud("cloudlol");
		(*view)->addPointCloud<pcl::PointXYZRGB>  (cloud, cloud_rgb, "cloudlol");
		(*view)->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloudlol");
		//(*view)->addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, normals);
		(*view)->spinOnce (100);
	//}
	#endif
	c++;
	//RGBDFrame * frame = new RGBDFrame(full_cloud);
	segment(full_cloud);
}

int main(int argc, char **argv)
{
	//pcl::visualization::CloudViewer viewer("Cloud Viewer");
	//view = &viewer;
	#ifdef build_full
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
	view = &viewer;
	viewer->setBackgroundColor (0, 0, 0);
	viewer->addCoordinateSystem (1.0);
	viewer->initCameraParameters ();
	camera = new RGBDCamera();
	mymap = new cvap_6D_slam::Map();
	mymap->setVisualization(viewer);
	#endif
	ros::init(argc, argv, "ekekrantz_slam");
	ros::NodeHandle n;
	#ifdef build_full
	ros::Subscriber sub  = n.subscribe("kinect_data", 100000, chatterCallback);
	ros::Subscriber sub2 = n.subscribe("/camera/depth_registered/points", 100000, cloudCallback);
	#endif
	ros::Subscriber sub3 = n.subscribe("/camera/rgb/points", 100000, cloudCallback);
	ros::spin();
	return 0;
}
